#include "stdint.h"
#include "pstwo.h"
#include "stm32f4xx_hal.h"

//手柄接口初始化    输入  DI->PB12
//                  输出  DO->PB13    CS->PB14  CLK->PB15

Joystick::Joystick()
{

}
//向手柄发送命令
void Joystick::send_cmd(uint8_t CMD)
{
	volatile uint16_t ref = 0x01;
	Data[1] = 0;
	for (ref = 0x01; ref < 0x0100; ref <<= 1)
	{
		if (ref & CMD)
		{
			DO_H;                   //输出以为控制位
		}
		else
			DO_L;

		CLK_H;                        //时钟拉高
		delay_us(CLOCK_PERIOD);
		CLK_L;
		delay_us(CLOCK_PERIOD);
		CLK_H;
		if (DI)
			Data[1] = ref | Data[1];
		delay_us(16);
	}
}
//判断是否为红灯模式
//返回值；0，红灯模式
//		  其他，其他模式
bool Joystick::is_red_light(void)
{
	CS_L;
	send_cmd(Comd[0]);  //开始命令
	send_cmd(Comd[1]);  //请求数据
	CS_H;
	if (Data[1] == 0x73)
		return true;
	else
		return false;

}
//读取手柄数据
void Joystick::read_data(void)
{
	volatile uint8_t byte = 0;
	volatile uint16_t ref = 0x01;

	CS_L;

	send_cmd(Comd[0]);  //开始命令
	send_cmd(Comd[1]);  //请求数据

	for (byte = 2; byte < 9; byte++)          //开始接受数据
	{
		for (ref = 0x01; ref < 0x100; ref <<= 1)
		{
			CLK_H;
			delay_us(CLOCK_PERIOD);
			CLK_L;
			delay_us(CLOCK_PERIOD);
			CLK_H;
			if (DI)
				Data[byte] = ref | Data[byte];
		}

		delay_us(CLOCK_PERIOD);
	}
	CS_H;
}

//对读出来的PS2的数据进行处理      只处理了按键部分         默认数据是红灯模式  只有一个按键按下时
//按下为0， 未按下为1。之前代码只识别一个按键，这里改成识别多个按键
void Joystick::data_key(uint8_t* p_array_result)
{
	uint8_t index;

	clear_data();
	read_data();
    // data_4 + data_3 = 16位
	Handkey = (Data[4] << 8) | Data[3];     //这是16个按键  按下为0， 未按下为1
	for (index = 0; index < 16; index++)
	{
		if ((Handkey & (1 << (MASK[index] - 1))) == 0)
			p_array_result[index] = index + 1;

	}

	// 去抖动
	debounce_button(p_array_result);


}

void Joystick::debounce_button(uint8_t* p_array_result)
{
	uint32_t now_ms = HAL_GetTick();
	for(uint8_t i =0; i< KEY_TOTAL_COUNT; i++)
	{
		if(p_array_result[i] != 0)
		{
			if(now_ms - key_last_ms[i] < 800)
			{
				//时间间隔< 100ms，取消这次按键
				p_array_result[i] = 0 ;
			}
			else
			{
				key_last_ms[i] = now_ms;

			}
		}

	}

}

//得到一个摇杆的模拟量	 范围0~256
uint8_t Joystick::anolog_data(uint8_t button)
{
	return Data[button];
}

//清除数据缓冲区
void Joystick::clear_data()
{
	uint8_t a;
	for (a = 0; a < 9; a++)
		Data[a] = 0x00;
}

void Joystick::delay_us(uint32_t us)
{
	// 调整此值以匹配实际需要的延迟时间
	// 这个系数需要根据你的MCU主频进行调整
	/*
	uint32_t delay = (SystemCoreClock / 8000000) * us;

	while (delay--)
	{
		__NOP(); // No operation instruction
	}
	*/

	uint32_t ticks;
	uint32_t told, tnow, tcnt = 0;
	uint32_t reload = SysTick->LOAD;  // 获取SysTick重载值（即最大计数值）

	ticks = us * (SystemCoreClock / 1000000);  // 计算需要等待的时钟周期数

	told = SysTick->VAL;  // 获取当前计数值

	while (1)
	{
		tnow = SysTick->VAL;
		if (tnow != told)
		{
			if (tnow < told)
			{
				tcnt += told - tnow;
			}
			else
			{
				tcnt += reload - tnow + told;
			}

			told = tnow;

			if (tcnt >= ticks)
			{
				break;
			}

		}

	}


}

// 振动设置
void Joystick::vibration_mode(void)
{
    CS_L;
    delay_us(16);
    send_cmd(0x01);
    send_cmd(0x4D);
    send_cmd(0X00);
    send_cmd(0x00);
    send_cmd(0X00);
	send_cmd(0X00);
	send_cmd(0x00);
	send_cmd(0X00);
	send_cmd(0X00);

    CS_H;
    delay_us(16);
}

// 完成并保存配置
void Joystick::exit_configing(void)
{
    CS_L;
    delay_us(16);
    send_cmd(0x01);
    send_cmd(0x43);
    send_cmd(0x00);
    send_cmd(0x00);
    send_cmd(0x5A);
    send_cmd(0x5A);
    send_cmd(0x5A);
    send_cmd(0x5A);
    send_cmd(0x5A);
    CS_H;
    delay_us(16);
}

// 手柄配置初始化
void Joystick::set_init(void)
{

    short_poll();
    short_poll();
    short_poll();
    enter_configing();      // 进入配置模式
    //turn_on_analog_mode();
    vibration_mode();       // 开启震动模式
    exit_configing();       // 完成并保存配置


}

void Joystick::vibration(uint8_t motor1, uint8_t motor2)
{
    CS_L;
    delay_us(16);
    send_cmd(0x01);  // 开始命令
    send_cmd(0x42);  // 请求数据
    send_cmd(0X00);

    //motor1:右侧小震动电机 0x00关，其他开
    //motor2:左侧大震动电机 0x40~0xFF 电机开，值越大 震动越大
    send_cmd(motor1);
    send_cmd(motor2);
    send_cmd(0X00);
    send_cmd(0X00);
    send_cmd(0X00);
    send_cmd(0X00);
    CS_H;
    delay_us(16);
}

void Joystick::short_poll(void)
{
    CS_L;
    delay_us(16);
    send_cmd(0x01);
    send_cmd(0x42);
    send_cmd(0x00);
    send_cmd(0x00);
    send_cmd(0x00);
    CS_H;
    delay_us(16);
}

// 进入配置
void Joystick::enter_configing(void)
{
    CS_L;
    delay_us(16);
    send_cmd(0x01);
    send_cmd(0x43);
    send_cmd(0X00);
    send_cmd(0x01);
    send_cmd(0x00);
    send_cmd(0x00);
    send_cmd(0x00);
    send_cmd(0x00);
    send_cmd(0x00);
    CS_H;
    delay_us(16);
}

// 发送模式设置
void Joystick::turn_on_analog_mode(void)
{
    CS_L;
    send_cmd(0x01);
    send_cmd(0x44);
    send_cmd(0X00);
    send_cmd(0x01); // analog=0x01; digital=0x00 软件设置发送模式
    send_cmd(0xEE); // 0x03 锁存设置，即不可通过按键 “MODE” 设置模式。
                   // 0xEE 不锁存软件设置，可通过按键 “MODE” 设置模式。
    send_cmd(0X00);
    send_cmd(0X00);
    send_cmd(0X00);
    send_cmd(0X00);
    CS_H;
    delay_us(16);
}
